#include "interface.h"

// Constructor must call the base class constructor.
Viewer::Viewer(QWidget *parent)
    : QGLViewer(parent)

{
    timeCode=0;
    points0loaded=false;
    animTime=20;
    animSpeed=80;
    animSpeedChanged(animSpeed);
    showCam0=showCam1=false; //default to not showing
    showTankGeom=true; //default to show tank geometry
    showTubing=true; //default to show tubing
    connect(this,SIGNAL(dataDoneSignal()),parent,SLOT(dataDone()));
}

#include<opencv2/opencv.hpp>
using namespace cv;

using namespace std;
void Viewer::draw()
{

    // Align the light with the view direction
    const qglviewer::Vec viewDir = camera()->viewDirection();
    const GLfloat dir[4] = {viewDir[0], viewDir[1], viewDir[2], 0};
    glLightfv(GL_LIGHT1, GL_POSITION, dir);

    if(showTubing) //  Draw the tubing
        drawTubes();

    if(showTankGeom) {//Draw Tank
    const float tankRadius = 309.6;
    const float nbSteps = 300.0;
    const float dTheta = 2.0*M_PI/nbSteps;


    glColor3f(0.5f, 0.5f , 0.5f);
    glBegin(GL_QUAD_STRIP);
    for (int i=0; i<nbSteps+1; ++i)
      {
        const float angle = (float)i*dTheta;
        const float c = cos(angle);
        const float s = sin(angle);
        const float z = s*tankRadius;
        const float y = c*tankRadius;

        glNormal3f(0, c, s);
        glVertex3f(0, y, z);
        glVertex3f(-550, y, z);
      }
    glEnd();

    //Draw the tank edges
     float lasty=tankRadius;
     float lastz=0;
    glColor3f(0.1f, 0.1f , 0.1f);
    glLineWidth(.5);
    glBegin(GL_LINES);
    for (int i=0; i<nbSteps+1; ++i)
    {
        const float angle = (float)i*dTheta;
        const float c = cos(angle);
        const float s = sin(angle);
        const float z = s*tankRadius;
        const float y = c*tankRadius;

        glVertex3f(0, lasty, lastz);
        glVertex3f(0, y, z);
        glVertex3f(-550, lasty, lastz);
        glVertex3f(-550, y, z);
        lasty=y;
        lastz=z;
    }
    glEnd();
    }

    //camera0 points
    if(!points0.empty() && showCam0){
        //Draw the scanned laser lines from Camera 0
        glColor3f(0.,1.,0.);
        glBegin(GL_POINTS);
        Mat_<Vec3f> _points=points0;

        for (int i=0; i<points0.rows; i++) {
            float x,y,z;
            x=_points(i)[0];
            y=_points(i)[1];
            z=_points(i)[2];

            glVertex3fv( qglviewer::Vec(x,y,z) );
        }
        glEnd();
    }

    //camera1 points
    if(!points1.empty() && showCam1){
        //Draw the scanned laser lines from Camera 1
        glColor3f(1.,1.,.4);
        glBegin(GL_POINTS);
        Mat_<Vec3f> _points=points1;

        for (int i=0; i<points1.rows; i++) {
            float x,y,z;
            x=_points(i)[0];
            y=_points(i)[1];
            z=_points(i)[2];

            glVertex3fv( qglviewer::Vec(x,y,z) );
        }
        glEnd();
    }

    if(!triangles.empty() && showTriangles) {
        //draw the triangles
        glColor3f(0.73,0.33,0.83); //medium orchid

        //calculate the vertex normals for each triangle
        Mat_<Vec3f> _triangles=triangles; //for short notation
        Mat_<Vec3f> _normals = Mat(1,triangles.rows,CV_32FC3);

        for (int i=0; i<triangles.rows; i+=3) {
            //vector v1, from the corner 0 to corner 1
            float v1x= _triangles(i+1)[0] -_triangles(i)[0];
            float v1y= _triangles(i+1)[1] -_triangles(i)[1];
            float v1z= _triangles(i+1)[2] -_triangles(i)[2];
            //vector v2, frm corner 0 to corner 2
            float v2x= _triangles(i+2)[0] -_triangles(i)[0];
            float v2y= _triangles(i+2)[1] -_triangles(i)[1];
            float v2z= _triangles(i+2)[2] -_triangles(i)[2];

            //cross product to get the triangle normal
            float normx= v1y*v2z - v1z*v2y;
            float normy= v1z*v2x - v1x*v2z;
            float normz= v1x*v2y - v1y*v2x;

            //normalize
            float mag=sqrt(normx*normx+normy*normy+normz*normz);
            normx /=mag;
            normy /=mag;
            normz /=mag;

            //set all thre vertices to the same normal (I think it might be called "hard shading")
            for(int j=0;j<3;j++) {
                _normals(i+j)[0]=normx;
                _normals(i+j)[1]=normy;
                _normals(i+j)[2]=normz;
            }
        }

        glBegin(GL_TRIANGLES);

        for (int i=0; i<triangles.rows; i++) {
            float x,y,z;

            x=_normals(i)[0];
            y=_normals(i)[1];
            z=_normals(i)[2];
            glNormal3f(x, y, z);

            x=_triangles(i)[0];
            y=_triangles(i)[1];
            z=_triangles(i)[2];
            glVertex3f(x, y, z);
        }
        glEnd();
    }
}


void Viewer::init()
{

    glEnable(GL_LIGHT1);

    // Light parameters, a dim white light
    const GLfloat light_ambient[4]  = {0.1, 0.1, 0.1, 1.0};
    const GLfloat light_specular[4] = {0.1, 0.1, 0.1, 1.0};
    const GLfloat light_diffuse[4]  = {0.1, 0.1, 0.1, 1.0};

    glLightfv(GL_LIGHT1, GL_AMBIENT,  light_ambient);
    glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
    glLightfv(GL_LIGHT1, GL_DIFFUSE,  light_diffuse);

    // Restore previous viewer state.
    restoreStateFromFile();
    setSceneRadius(600.0);
    setSceneCenter(qglviewer::Vec(-250,0,0));
    camera()->showEntireScene();
    setAnimationPeriod(animTime);

}

QString Viewer::helpString() const
{
  QString text("<h2>Slosh-Test Viewer</h2>");
  text += "Use the mouse to move the camera around the object. ";
  text += "You can respectively revolve around, zoom and translate with the three mouse buttons. ";
  text += "Left and middle buttons pressed together rotate around the camera view direction axis<br><br>";
  text += "Pressing <b>Alt</b> and one of the function keys (<b>F1</b>..<b>F12</b>) defines a camera keyFrame. ";
  text += "Simply press the function key again to restore it. Several keyFrames define a ";
  text += "camera path. Paths are saved when you quit the application and restored at next start.<br><br>";
  text += "Press <b>F</b> to display the frame rate, <b>A</b> for the world axis, ";
  text += "<b>Alt+Return</b> for full screen mode and <b>Control+S</b> to save a snapshot. ";
  text += "See the <b>Keyboard</b> tab in this window for a complete shortcut list.<br><br>";
  text += "Double clicks automates single click actions: A left button double click aligns the closer axis with the camera (if close enough). ";
  text += "A middle button double click fits the zoom of the camera and the right button re-centers the scene.<br><br>";
  text += "A left button double click while holding right button pressed defines the camera <i>Revolve Around Point</i>. ";
  text += "See the <b>Mouse</b> tab and the documentation web pages for details.<br><br>";
  text += "Press <b>Escape</b> to exit the viewer.";
  return text;
}


void Viewer::animate()
{
    if(trianglesLoaded && points0loaded && points1loaded){ //the way I have this written both point sets need to be loaded
        if(animSpeed>0){
            it++; //iterate cam0
            it1++; //iterate cam1
            itTriangles++; //iterate triangles

            if(it==it_end || it1 ==it_end1 || itTriangles==itTrianglesEnd) { //need to wind to the front
                it=frames.begin();
                it1=frames1.begin();
                itTriangles=framesTriangles.begin();
            }
        } else {
            it--; //iterate cam0
            it1--; //iterate cam0
            itTriangles--; //iterate triangles

            if(it == it_begin || it1 == it_begin1 || itTriangles ==itTrianglesBegin) { //need to wind to the back
                it=frames.end();
                it1=frames1.end();
                itTriangles=framesTriangles.end();
            }
        }

        (*it)["ExportedPoints"] >> points0;
        (*it1)["ExportedPoints"] >> points1;
        (*itTriangles)["Triangles"] >> triangles;
        (*itTriangles)["TimeCode"] >> timeCode;

        int curPos=100-it.remaining*100/frames.size();

        emit currentPosition(curPos);
        emit currentTimecode(timeCode);
    }
}

void Viewer::loadResultFile()
{   //this stub function calls the real function in a separate thread (avoids blocking the GUI)
   run(this, &Viewer::loadResultFile_real);  //note to self - not sure why I did this.
                                             //It seems like I could have just used concurrent run from where I call this function
                                             //instead of using this stub.  I don't have time to look at it, and it works as it is.
}

void Viewer::loadResultFile_real()
{
    fsTriangles.open(triangleFileName.toStdString(), FileStorage::READ);
    if (!fsTriangles.isOpened())
        qDebug() << "Failed to open data file: " << triangleFileName;
    else {
        //set up sequence and iterator
        framesTriangles = fsTriangles["Frames"];
        itTriangles = itTrianglesBegin = framesTriangles.begin();
        itTrianglesEnd = framesTriangles.end();
        //read the triangles
        (*itTriangles)["Triangles"] >> triangles;
        if(!triangles.empty()) {
            trianglesLoaded=true;
            showTriangles=true;
        }
    }


    fs.open(dataFile0.toStdString(), FileStorage::READ);
    if (!fs.isOpened())
    {
        qDebug() << "Failed to open data file: "<< dataFile0;

    } else {

        // setup sequence & iterator
        frames = fs["Frames"];
        it = it_begin = frames.begin();
        it_end = frames.end();

        //read in the points
        (*it)["ExportedPoints"] >> points0;

        if (!points0.empty()){
            points0loaded=true;
            showCam0=true;
        }
    }

    fs1.open(dataFile1.toStdString(), FileStorage::READ);
    if (!fs1.isOpened())
    {
        qDebug() << "Failed to open data file: "<< dataFile1;

    } else {

        // setup sequence & iterator
        frames1 = fs1["Frames"];
        it1 = it_begin1 = frames1.begin();
        it_end1 = frames1.end();

        //read in the points
        (*it1)["ExportedPoints"] >> points1;

        if (!points1.empty()){
            points1loaded=true;
            showCam1=true;
        }
    }

    emit dataDoneSignal();
}

void Viewer::animSpeedChanged(int speed)
{

    animSpeed=speed;
    animTime=500-5*abs(speed);

    if( animationIsStarted() ) {
        stopAnimation();
        setAnimationPeriod(animTime);
        startAnimation();
    }
    else  setAnimationPeriod(animTime);
}


void drawCylinder(float x1, float y1, float z1, float x2,float y2, float z2, float radius,int subdivisions)
{
    //cut and pasted with modifications from:
    // https://github.com/curran/renderCyliner/blob/master/renderCylinder.c

  GLUquadricObj *quadric=gluNewQuadric();
  gluQuadricNormals(quadric, GLU_SMOOTH);

  float vx = x2-x1;
  float vy = y2-y1;
  float vz = z2-z1;
  float v = sqrt( vx*vx + vy*vy + vz*vz );
  float ax;

  if (fabs(vz) < 1.0e-3) {
      ax = 57.2957795*acos( vx/v ); // rotation angle in x-y plane
      if ( vy <= 0.0 )
          ax = -ax;
  }
  else {
      ax = 57.2957795*acos( vz/v ); // rotation angle
      if ( vz <= 0.0 )
          ax = -ax;
  }

  float rx = -vy*vz;
  float ry = vx*vz;

  glPushMatrix();
  //draw the cylinder body
  glTranslatef( x1,y1,z1 );
  if (fabs(vz) < 1.0e-3) {
      glRotated(90.0, 0, 1, 0.0); // Rotate & align with x axis
      glRotated(ax, -1.0, 0.0, 0.0); // Rotate to point 2 in x-y plane
  }
  else {
      glRotated(ax, rx, ry, 0.0); // Rotate about rotation vector
  }
  gluQuadricOrientation(quadric,GLU_OUTSIDE);
  gluCylinder(quadric, radius, radius, v, subdivisions, 1);
  glPopMatrix();

  gluDeleteQuadric(quadric);
}

void Viewer::drawTubes()
{
    int nPoints=10; //number points per tube

    float tubePointsBig[] = {316.8,-148.1, 292.7, 316.8,-33.3, 22.2, 316.8, -30.4, 13.1, 316.8, 21.2, -224.2, 314.3, 24.5, -239.6,
                             307.0, 27.5, -253.5, 295.8, 29.9,-264.5, 281.5, 31.5, -271.6, 265.8, 32.0, -274.0, 124.8, 32.0, -274.0};

    float tubePointsSmall[] = {253.3,-148.1,292.7, 253.3,-32.6,20.6, 253.3,-30.6,14.94, 253.3,39.3,-225.0, 255.8, 43.7, -240.2,
                               263.0, 47.7, -253.9, 274.3, 50.84, -264, 288.5, 52.9, -271.6, 304.3,53.6,-274.0, 331.3,53.6,-274.0};

    float tubeRadiusBig=11.8;
    float tubeRadiusSmall=10.2;

    // position the tube assembly. Adjusting point data that came from the CAD geometry to match my test fixture geom
    glPushMatrix();
    glTranslatef(-487.6, 0, 0);  //translate in the x-direction

    glColor3f(0.2f, 0.2f , 0.2f);

    //draw the big tube
    for(int i=0;i<(nPoints-1)*3;i+=3)
        drawCylinder(tubePointsBig[i],tubePointsBig[i+1],tubePointsBig[i+2], tubePointsBig[i+3],tubePointsBig[i+4],tubePointsBig[i+5], tubeRadiusBig, 25);

    //draw the small tube
    for(int i=0;i<(nPoints-1)*3;i+=3)
        drawCylinder(tubePointsSmall[i],tubePointsSmall[i+1],tubePointsSmall[i+2], tubePointsSmall[i+3],tubePointsSmall[i+4],tubePointsSmall[i+5], tubeRadiusSmall, 25);

      glPopMatrix(); //back to normal

}
